#include <QtGui>
#include <iostream>

#include <string>
#include <QString>
#include <sstream>
#include <fstream>

#include <time.h>

#include <QTimer>
#include <QPainter>
#include <QImage>
#include <QPixmap>
#include <QGraphicsScene>

#include <cmath>

#include <sys/stat.h> 

#define TIMER 20

#include "gui.h"

Gui::Gui(QMainWindow *parent) : QMainWindow(parent)
{
  ui.setupUi(this);
  
  udpSocket = new QUdpSocket(this);
//  udpSocket->bind(QHostAddress::Any, 7755);
//  udpSocket->bind(QHostAddress("192.168.1.107"), 7755);
  udpSocket->bind(7755, QUdpSocket::ShareAddress);
    
  connect(udpSocket, SIGNAL(readyRead()),
             this, SLOT(readPendingDatagrams()));  
             
  timerId = startTimer(TIMER); 
  
  for(int i = 0; i < 6; i++)
  {
    robots[i] = new Status(QHostAddress::Any,(quint16)0);
  }
  
  if(FileExists("/dev/input/js0"))
  {
    joy = new Joystick("/dev/input/js0");
    hasJoyStick = true;
  }
  else
  {
    hasJoyStick = false;
    std::cout << "Could not find controller" << std::endl;
  }  
  
  robRotation = 0;
  
  ui.compass->scale(.5,.5);
  
  ui.compass->setRenderHints(QPainter::Antialiasing | QPainter::SmoothPixmapTransform);
  ui.robotGraphics->setRenderHints(QPainter::Antialiasing | QPainter::SmoothPixmapTransform);
  ui.logo->setRenderHints(QPainter::Antialiasing | QPainter::SmoothPixmapTransform);    

  logoPmap = new QPixmap("resources/logo.jpg");
  logoGraphicsScene = new QGraphicsScene();
  logoGraphicsScene->addPixmap(*logoPmap);
  ui.logo->setScene(logoGraphicsScene);
  
  compassPmap = new QPixmap("resources/compass.jpg");
  compassGraphicsScene = new QGraphicsScene();
  compassGraphicsScene->addPixmap(*compassPmap);
  
  QPen pen(QBrush(QColor(0,0,0)),6);
  
  double x1 = compassGraphicsScene->width()/2,
         y1 = compassGraphicsScene->height()/2;
  QLineF compassNeedle(x1, y1, x1, y1-180);
  
  compassGraphicsScene->addLine(compassNeedle,pen);
  ui.compass->setScene(compassGraphicsScene);
  
  robotPmap = new QPixmap("resources/robot.jpg");
  robotGraphicsScene = new QGraphicsScene();
  robotGraphicsScene->addPixmap(*robotPmap);
  ui.robotGraphics->setScene(robotGraphicsScene);
  
  imuControl = false;
  debounced = true;
  
  connect( ui.calButton, SIGNAL( clicked() ), this, SLOT( calibrateDepth() ) ); 

  time_t rawtime;
  struct tm * timeinfo;
  time ( &rawtime );
  timeinfo = localtime ( &rawtime );
  string filename;
  std::stringstream out;
  out << timeinfo->tm_mon << "." << timeinfo->tm_mday << "." << timeinfo->tm_hour << "." << timeinfo->tm_min << "." << timeinfo->tm_sec;
  filename = out.str();
  
  datafile.open(filename.c_str());
  datafile << "Seconds Yaw Pitch Roll";
}

Gui::~Gui()
{
	datafile.close();
}

bool Gui::FileExists(string strFilename) {
  struct stat stFileInfo;
  bool blnReturn;
  int intStat;

  // Attempt to get the file attributes
  intStat = stat(strFilename.c_str(),&stFileInfo);
  if(intStat == 0) {
    // We were able to get the file attributes
    // so the file obviously exists.
    blnReturn = true;
  } else {
    // We were not able to get the file attributes.
    // This may mean that we don't have permission to
    // access the folder which contains this file. If you
    // need to do that level of checking, lookup the
    // return values of stat which will give you
    // more details on why stat failed.
    blnReturn = false;
  }
  
  return(blnReturn);
}

void Gui::calibrateDepth()
{
  if(ui.radio1->isChecked())
  {
    robots[0]->setAmbientPressure();
  }
  else if(ui.radio2->isChecked())
  {
    robots[1]->setAmbientPressure();
  }
  else if(ui.radio3->isChecked())
  {
    robots[2]->setAmbientPressure();
  }
  else if(ui.radio4->isChecked())
  {
    robots[3]->setAmbientPressure();
  }
  else if(ui.radio5->isChecked())
  {
    robots[4]->setAmbientPressure();
  }
  else if(ui.radio6->isChecked())
  {
    robots[5]->setAmbientPressure();
  }
}

void Gui::getJoyStickCommands()
{
//Joystick routine
  //Axes
  //------
  //Axis 0 - left stick "X axis" (- to left, + to right)
  //Axis 1 - left stick "Y axis" (- upward, + downward)
  //Axis 2 - right stick "X axis" (- to left, + to right)
  //Axis 3 - right stick "Y axis" (- upward, + downward)
  //Axis 4 - left tilt accelerometer (- counterclockwise, + clockwise)
  //Axis 5 - forward tilt accelerometer (- away, + toward)
  //Axis 6 - z accelerometer (- upright, + upside down)
  //Axis 8 - up D pad (0 to 32767)
  //Axis 9 - right D pad
  //Axis 10 - down D pad
  //Axis 11 - left D pad
  //Axis 12 - L2
  //Axis 13 - R2
  //Axis 14 - L1
  //Axis 15 - R1
  //Axis 16 - Triangle
  //Axis 17 - Circle
  //Axis 18 - X
  //Axis 19 - Square
  //--------
  //Buttons
  //--------
  // 0 - Select
  // 1 - L3
  // 2 - R3
  // 3 - Start
  // 4 - up d pad
  // 5 - right d pad
  // 6 - down d pad
  // 7 - left d pad
  // 8 - L2
  // 9 - R2
  // 10 - L1
  // 11 - R1
  // 12 - triangle
  // 13 - circle
  // 14 - x
  // 15 - square
  // 16 - PS
  double lateral, forward, spin, depth;
  
  lateral = joy->getAxis(0)/32767.0*80;
  forward = joy->getAxis(1)/32767.0*-80;
  spin = joy->getAxis(2)/32767.0*35;
  depth = joy->getAxis(3)/32767.0*-80;
  
  int servo = 0;
  
  if(joy->getAxis(14) > 5000)
    servo = 1;
  else if(joy->getAxis(12) > 5000)
    servo = -1;
    
  int tri,squ,cir;

  if(joy->getAxis(16)> 5000)
    tri = 1;
  else if(joy->getAxis(8)> 5000)
    tri = -1;
  else
    tri = 0;
    
  if(joy->getAxis(19)> 5000)
    squ = 1;
  else if(joy->getAxis(11)> 5000)
    squ = -1;
  else
    squ = 0;
    
  if(joy->getAxis(17)> 5000)
    cir = 1;
  else if(joy->getAxis(9)> 5000)
    cir = -1;
  else
    cir = 0;  
    
  char selectButton = joy->getButton(0);
  
  if(selectButton == 1 && debounced)
  {
    if(!imuControl)
      imuControl = true;
    else
      imuControl = false;
    
    setImuControl(imuControl);
    debounced = false;
  }
  else if(selectButton == 0 && !debounced)
  {
    debounced = true;
  }
  
  sendJoy(lateral,forward,spin,depth,servo,tri,cir,squ);
}

void Gui::timerEvent(QTimerEvent *event)
{
  if(hasJoyStick)
    getJoyStickCommands(); 
  
  //Keepalive routine
  for(int i = 0; i < 6; i++)
  {
    if(!robots[i]->isConnected())
      continue;
      
    int timeAck = robots[i]->Alive();
    if(timeAck > 100)
    {
      robots[i]->setConnected(false);
    }
  }
  
  ui.rob_1->setValue(robots[0]->isConnected());
  ui.rob_2->setValue(robots[1]->isConnected());
  ui.rob_3->setValue(robots[2]->isConnected());
  ui.rob_4->setValue(robots[3]->isConnected());
  ui.rob_5->setValue(robots[4]->isConnected());
  ui.rob_6->setValue(robots[5]->isConnected());
  
  ui.radio1->setEnabled(robots[0]->isConnected());
  ui.radio2->setEnabled(robots[1]->isConnected());
  ui.radio3->setEnabled(robots[2]->isConnected());
  ui.radio4->setEnabled(robots[3]->isConnected());
  ui.radio5->setEnabled(robots[4]->isConnected());
  ui.radio6->setEnabled(robots[5]->isConnected());
  
  double motorData[18];
  
  double currentDepth = 0;
  
  if(ui.radio1->isChecked())
  {
    ui.yawLabel  ->setText(QString("%1").arg(robots[0]->getYaw()));  
    ui.pitchLabel->setText(QString("%1").arg(robots[0]->getPitch()));
    ui.rollLabel ->setText(QString("%1").arg(robots[0]->getRoll())); 
    ui.tempReadout ->display(robots[0]->getTemperature());      
    ui.pressReadout ->display(robots[0]->getPressure());          
    
    robots[0]->getMotor(1,&motorData[0]);
    robots[0]->getMotor(2,&motorData[3]);
    robots[0]->getMotor(3,&motorData[6]);
    robots[0]->getMotor(4,&motorData[9]);
    robots[0]->getMotor(5,&motorData[12]);
    robots[0]->getMotor(6,&motorData[15]);
    
    currentDepth = robots[0]->getDepth();

    if(robots[0]->isConnected()){
      time_t seconds;
      seconds = time(NULL);
      datafile << seconds << " " << robots[0]->getYaw() << " " << robots[0]->getPitch() << " " << robots[0]->getRoll() << std::endl ; 
    }
  }
  else if(ui.radio2->isChecked())
  {
    ui.yawLabel  ->setText(QString("%1").arg(robots[1]->getYaw()));  
    ui.pitchLabel->setText(QString("%1").arg(robots[1]->getPitch()));
    ui.rollLabel ->setText(QString("%1").arg(robots[1]->getRoll()));
    ui.tempReadout ->display(robots[1]->getTemperature());      
    ui.pressReadout ->display(robots[1]->getPressure());               
    
    robots[1]->getMotor(1,&motorData[0]);
    robots[1]->getMotor(2,&motorData[3]);
    robots[1]->getMotor(3,&motorData[6]);
    robots[1]->getMotor(4,&motorData[9]);
    robots[1]->getMotor(5,&motorData[12]);
    robots[1]->getMotor(6,&motorData[15]);    
    
    currentDepth = robots[1]->getDepth();
  }
  else if(ui.radio3->isChecked())
  {
    ui.yawLabel  ->setText(QString("%1").arg(robots[2]->getYaw()));  
    ui.pitchLabel->setText(QString("%1").arg(robots[2]->getPitch()));
    ui.rollLabel ->setText(QString("%1").arg(robots[2]->getRoll()));   
    ui.tempReadout ->display(robots[2]->getTemperature());      
    ui.pressReadout ->display(robots[2]->getPressure());                    
    
    robots[2]->getMotor(1,&motorData[0]);
    robots[2]->getMotor(2,&motorData[3]);
    robots[2]->getMotor(3,&motorData[6]);
    robots[2]->getMotor(4,&motorData[9]);
    robots[2]->getMotor(5,&motorData[12]);
    robots[2]->getMotor(6,&motorData[15]);
    
    currentDepth = robots[2]->getDepth();
  }
  else if(ui.radio4->isChecked())
  {
    ui.yawLabel  ->setText(QString("%1").arg(robots[3]->getYaw()));  
    ui.pitchLabel->setText(QString("%1").arg(robots[3]->getPitch()));
    ui.rollLabel ->setText(QString("%1").arg(robots[3]->getRoll()));   
    ui.tempReadout ->display(robots[3]->getTemperature());      
    ui.pressReadout ->display(robots[3]->getPressure());                    
    
    robots[3]->getMotor(1,&motorData[0]);
    robots[3]->getMotor(2,&motorData[3]);
    robots[3]->getMotor(3,&motorData[6]);
    robots[3]->getMotor(4,&motorData[9]);
    robots[3]->getMotor(5,&motorData[12]);
    robots[3]->getMotor(6,&motorData[15]);
    
    currentDepth = robots[3]->getDepth();
  }
  else if(ui.radio5->isChecked())
  {
    ui.yawLabel  ->setText(QString("%1").arg(robots[4]->getYaw()));  
    ui.pitchLabel->setText(QString("%1").arg(robots[4]->getPitch()));
    ui.rollLabel ->setText(QString("%1").arg(robots[4]->getRoll()));   
    ui.tempReadout ->display(robots[4]->getTemperature());      
    ui.pressReadout ->display(robots[4]->getPressure());                    
    
    robots[4]->getMotor(1,&motorData[0]);
    robots[4]->getMotor(2,&motorData[3]);
    robots[4]->getMotor(3,&motorData[6]);
    robots[4]->getMotor(4,&motorData[9]);
    robots[4]->getMotor(5,&motorData[12]);
    robots[4]->getMotor(6,&motorData[15]);
    
    currentDepth = robots[4]->getDepth();
  }
  else if(ui.radio6->isChecked())
  {
    ui.yawLabel  ->setText(QString("%1").arg(robots[5]->getYaw()));  
    ui.pitchLabel->setText(QString("%1").arg(robots[5]->getPitch()));
    ui.rollLabel ->setText(QString("%1").arg(robots[5]->getRoll()));   
    ui.tempReadout ->display(robots[5]->getTemperature());      
    ui.pressReadout ->display(robots[5]->getPressure());                    
    
    robots[5]->getMotor(1,&motorData[0]);
    robots[5]->getMotor(2,&motorData[3]);
    robots[5]->getMotor(3,&motorData[6]);
    robots[5]->getMotor(4,&motorData[9]);
    robots[5]->getMotor(5,&motorData[12]);
    robots[5]->getMotor(6,&motorData[15]);
    
    currentDepth = robots[5]->getDepth();  
  }
  ui.scv1MPower ->setValue(motorData[0]);
  ui.scv1TempBar->setValue(motorData[1]);
  ui.scv1Vin     ->display(motorData[2]);  
  
  ui.scv2MPower ->setValue(motorData[3]);
  ui.scv2TempBar->setValue(motorData[4]);
  ui.scv2Vin     ->display(motorData[5]);

  ui.scv3MPower ->setValue(motorData[6]);
  ui.scv3TempBar->setValue(motorData[7]);
  ui.scv3Vin     ->display(motorData[8]);
  
  ui.scf1MPower ->setValue(motorData[9]);
  ui.scf1TempBar->setValue(motorData[10]);
  ui.scf1Vin     ->display(motorData[11]);
  
  ui.scf2MPower ->setValue(motorData[12]);
  ui.scf2TempBar->setValue(motorData[13]);
  ui.scf2Vin     ->display(motorData[14]);
  
  ui.sclMPower ->setValue(motorData[15]);
  ui.sclTempBar->setValue(motorData[16]);
  ui.sclVin     ->display(motorData[17]);
  
  ui.depthMeter ->display(currentDepth);               
  
  renderRobot();
  renderCompass();
  
  repaint();
}

void Gui::renderCompass()
{    
  double x1 = compassGraphicsScene->width()/2,
         y1 = compassGraphicsScene->height()/2,
         x2,y2,compAng;
         
  if(ui.radio1->isChecked())
  {
    compAng = robots[0]->getYaw();
  }
  else if(ui.radio2->isChecked())
  {
    compAng = robots[1]->getYaw();
  }
  else if(ui.radio3->isChecked())
  {
    compAng = robots[2]->getYaw();
  }
  else if(ui.radio4->isChecked())
  {
    compAng = robots[3]->getYaw();
  }
  else if(ui.radio5->isChecked())
  {
    compAng = robots[4]->getYaw();
  }
  else if(ui.radio6->isChecked())
  {
    compAng = robots[5]->getYaw();
  }
         
  x2 = x1+180*sin(compAng/180*3.14);
  y2 = y1-180*cos(compAng/180*3.14);
  
  //change line
  compassNeedle.setLine(x1,y1,x2,y2);
  delete compassGraphicsScene;
  compassGraphicsScene = new QGraphicsScene();
  compassGraphicsScene->addPixmap(*compassPmap);
  compassGraphicsScene->addLine(compassNeedle,pen);
  ui.compass->setScene(compassGraphicsScene);
}

void Gui::renderRobot()
{
  double roll = 0;
  double pitch = 0;
  
  if(ui.radio1->isChecked())
  {
    roll = robots[0]->getRoll();
    pitch = robots[0]->getPitch();
  }
  else if(ui.radio2->isChecked())
  {
    roll = robots[1]->getRoll();
    pitch = robots[1]->getPitch();
  }
  else if(ui.radio3->isChecked())
  {
    roll = robots[2]->getRoll();
    pitch = robots[2]->getPitch();
  }
  else if(ui.radio4->isChecked())
  {
    roll = robots[3]->getRoll();
    pitch = robots[3]->getPitch();
  }
  else if(ui.radio5->isChecked())
  {
    roll = robots[4]->getRoll();
    pitch = robots[4]->getPitch();
  }
  else if(ui.radio6->isChecked())
  {
    roll = robots[5]->getRoll();
    pitch = robots[5]->getPitch();
  }
  
  
  double rotateSet = roll;  
  double rotateAmount = rotateSet - robRotation;
  
  ui.robotGraphics->rotate(rotateAmount);
  
  robRotation += rotateAmount;
}

void Gui::readPendingDatagrams()
{
  while (udpSocket->hasPendingDatagrams()) {
    QByteArray datagram;
    datagram.resize(udpSocket->pendingDatagramSize());
    QHostAddress sender;
    quint16 senderPort;

    udpSocket->readDatagram(datagram.data(), datagram.size(),
                              &sender, &senderPort);
                                 
    QDataStream in(&datagram, QIODevice::ReadOnly);
    in.setVersion(QDataStream::Qt_4_0);
       
    quint16 size;
    QString data;
       
    in >> size;
    in >> data;
       
    if(data == QString("reqConnect"))
    {
      int index;
      in >> index;
      
      robots[index] = new Status(sender,senderPort); 
      robots[index]->setConnected(true);
      
      sendAck(sender,senderPort);
    }
    else if(data == QString("Alive"))
    {
      int index;
      in >> index;
      
      robots[index]->setConnected(true);      
      robots[index]->update();
      
      sendAck(sender,senderPort);
    }
    else if(data == QString("motor"))
    {
      int index;
      in >> index;
      
      for(int i = 1; i <= 6; i++)
      {
        double speed, temp, vin;

        in >> speed;
        in >> temp;
        in >> vin;
        
        robots[index]->setMotor(i,speed,temp,vin);
        
      }
      
      robots[index]->setConnected(true);      
      robots[index]->update();
    }
    else if(data == QString("data"))
    {
      int index;
      in >> index;
      
      robots[index]->setConnected(true);      
      robots[index]->update();
      
      double yaw,pitch,roll,temp,pressure;
      
      in >> yaw;
      in >> pitch;
      in >> roll;
      in >> temp;
      in >> pressure;
      
      robots[index]->setOrientation(roll,pitch,yaw);
      robots[index]->setTemperature(temp);
      robots[index]->setPressure(pressure);
    }
  }
}

void Gui::setImuControl(bool imuControl)
{
  QHostAddress hostaddress;
  quint16 port;
  
  if(ui.radio1->isChecked())
  {
    hostaddress = robots[0]->getHost();
    port = robots[0]->getPort();
  }
  else if(ui.radio2->isChecked())
  {
    hostaddress = robots[1]->getHost();
    port = robots[1]->getPort();
  }
  else if(ui.radio3->isChecked())
  {
    hostaddress = robots[2]->getHost();
    port = robots[2]->getPort();
  }
  else if(ui.radio4->isChecked())
  {
    hostaddress = robots[3]->getHost();
    port = robots[3]->getPort();
  }
  else if(ui.radio5->isChecked())
  {
    hostaddress = robots[4]->getHost();
    port = robots[4]->getPort();
  }
  else if(ui.radio6->isChecked())
  {
    hostaddress = robots[5]->getHost();
    port = robots[5]->getPort();
  }
  
  QByteArray block;
  QDataStream out(&block, QIODevice::WriteOnly);
  out.setVersion(QDataStream::Qt_4_0);
  out << (quint16)0;
  out << QString("IMUControl");
  out << imuControl;
  out.device()->seek(0);
  out << (quint16)(block.size() - sizeof(quint16));

  udpSocket->writeDatagram(block,hostaddress,port);
}

void Gui::sendJoy(double lateral, double forward, double spin, double depth, int servo, int u1, int u2, int u3)
{

  QHostAddress hostaddress;
  quint16 port;
  
  if(ui.radio1->isChecked())
  {
    hostaddress = robots[0]->getHost();
    port = robots[0]->getPort();
  }
  else if(ui.radio2->isChecked())
  {
    hostaddress = robots[1]->getHost();
    port = robots[1]->getPort();
  }
  else if(ui.radio3->isChecked())
  {
    hostaddress = robots[2]->getHost();
    port = robots[2]->getPort();
  }
  else if(ui.radio4->isChecked())
  {
    hostaddress = robots[3]->getHost();
    port = robots[3]->getPort();
  }
  else if(ui.radio5->isChecked())
  {
    hostaddress = robots[4]->getHost();
    port = robots[4]->getPort();
  }
  else if(ui.radio6->isChecked())
  {
    hostaddress = robots[5]->getHost();
    port = robots[5]->getPort();
  }
  
  QByteArray block;
  QDataStream out(&block, QIODevice::WriteOnly);
  out.setVersion(QDataStream::Qt_4_0);
  out << (quint16)0;
  out << QString("Joystick");
  out << lateral;
  out << forward;
  out << spin;
  out << servo;
  out << depth;
  out << u1;
  out << u2;
  out << u3;
  out.device()->seek(0);
  out << (quint16)(block.size() - sizeof(quint16));

  udpSocket->writeDatagram(block,hostaddress,port);
}

void Gui::sendAck(QHostAddress hostaddress, quint16 port)
{
  QByteArray block;
  QDataStream out(&block, QIODevice::WriteOnly);
  out.setVersion(QDataStream::Qt_4_0);
  out << (quint16)0;
  out << QString("ackConnect");
  out.device()->seek(0);
  out << (quint16)(block.size() - sizeof(quint16));

  udpSocket->writeDatagram(block,hostaddress,port);
}
